#include <irrlicht.h>
#include <vector>
#include "GraphNode.h"
#include "GraphEdge.h"

bool ValidNeighbour(int x, int y, int NumCellsX, int NumCellsY)
{
  return !((x < 0) || (x >= NumCellsX) || (y < 0) || (y >= NumCellsY));
}

template <class graph_type>
void GraphHelper_AddAllNeighboursToGridNode(graph_type& graph, int row, int col, int NumCellsX, int NumCellsY)
{   
  for (int i=-1; i<2; ++i)
  {
    for (int j=-1; j<2; ++j)
    {
      int nodeX = col+j;
      int nodeY = row+i;

      //skip if equal to this node
      if ( (i == 0) && (j==0) ) continue;

      //check to see if this is a valid neighbour
      if (ValidNeighbour(nodeX, nodeY, NumCellsX, NumCellsY))
      {
        //calculate the distance to this node
        Vector2D PosNode      = graph.GetNode(row*NumCellsX+col).Pos();
        Vector2D PosNeighbour = graph.GetNode(nodeY*NumCellsX+nodeX).Pos();

        double dist = PosNode.Distance(PosNeighbour);

        //this neighbour is okay so it can be added
        graph_type::EdgeType NewEdge(row*NumCellsX+col,
                                     nodeY*NumCellsX+nodeX,
                                     dist);
        graph.AddEdge(NewEdge);

        //if graph is not a diagraph then an edge needs to be added going
        //in the other direction
        if (!graph.isDigraph())
        {
          graph_type::EdgeType NewEdge(nodeY*NumCellsX+nodeX,
                                       row*NumCellsX+col,
                                       dist);
          graph.AddEdge(NewEdge);
        }
      }
    }
  }
}

template <class graph_type>
void CreateGrid(graph_type& graph, core::rectf floor, int numberCellsX, int numberCellsY)
{
	core::dimension2df ul_orgin = floor.UpperLeftCorner;
	
	double CellWidth  = (double)floor.getWidth() / (double)numberCellsX;
	double CellHeight = (double)floor.getHeight() / (double)numberCellsY;

	double midX = CellWidth/2;
	double midY = CellHeight/2;

	for (int row=0; row<numberCellsY; ++row)
	{
		for (int col=0; col<numberCellsX; ++col)
		{
			core::vector2df vec(ul_orgin.Width + (midX + (col*CellWidth)),ul_orgin.Height +( midY + (row*CellHeight)));
			printf("Add node with point < %f, %f >\n", vec.X, vec.Y);
			graph.addNode(NavGraphNode<>(graph.getNextFreeNodeIndex(),vec));
		}
	}

	for(int row=0; row<numberCellsY; ++row)
	{
		for(int col=0; col<numberCellsX; ++col)
		{
			AddAllNeighboursToGridNode(graph, row, col, numberCellsX, numberCellsY);
		}
	}

	printf("Initialized graph with %i nodes and %i edges\n", graph.getNodeCount(), graph.getEdgeCount());
}

template <class graph_type>
void AddAllNeighboursToGridNode(graph_type& graph, 
	int row, int col, int NumCellsX, int NumCellsY)
{   

	for (int i=-1; i<2; ++i)
	{
		///
		for (int j=-1; j<2; ++j)
		{
			int nodeX = col+j;
			int nodeY = row+i;

			//skip if equal to this node
			if ( (i == 0) && (j==0) ) continue;

			//check to see if this is a valid neighbour
			if (ValidNeighbour(nodeX, nodeY, NumCellsX, NumCellsY))
			{
				//calculate the distance to this node
				core::vector2df PosNode = graph.getNode(row*NumCellsX+col).getPosition();
				core::vector2df PosNeighbour = graph.getNode(nodeY*NumCellsX+nodeX).getPosition();

				double dist = PosNode.getDistanceFrom(PosNeighbour);

				//this neighbour is okay so it can be added
				graph_type::EdgeType NewEdge(row*NumCellsX+col, nodeY*NumCellsX+nodeX, dist);
				graph.addEdge(NewEdge);

				//if graph is not a diagraph then an edge needs to be added going
				//in the other direction
				if (!graph.isDigraph())
				{
					graph_type::EdgeType NewEdge(nodeY*NumCellsX+nodeX,
												row*NumCellsX+col, dist);
					graph.addEdge(NewEdge);
				}
			}
		}
	}
}

template <class graph_type>
void Graph_AddObstacle(graph_type& graph, aabbox3df obstacle)
{
	std::vector<int> deletedNodes;
	vector2df nodePosition;

	graph_type::NodeIterator iter(graph);
	for(NavGraphNode<>* pN = iter.begin(); !iter.end(); pN=iter.next())
	{
		nodePosition = pN->getPosition();
		vector3df testPoint(nodePosition.X, 20, nodePosition.Y);
		if(obstacle.isPointInside(testPoint))
		{
			printf("GraphNode %i at position < %f %f > needs to be removed\n", pN->getIndex(), nodePosition.X, nodePosition.Y);
			deletedNodes.push_back(	pN->getIndex() );
		}
	}

	for(std::vector<int>::iterator ndIter = deletedNodes.begin(); ndIter != deletedNodes.end(); ++ndIter)
	{
		//printf("Removeing node %i\n", *ndIter );
		graph.removeNode(*ndIter);
	}

	printf("%i Nodes successfully removed\n", deletedNodes.size());
}

template <class graph_type>
int Graph_FindNodeIdContainingVector(graph_type& graph, core::vector2df position, core::rectf floor, int numberCellsX, int numberCellsY)
{
	
	core::dimension2df ul_orgin = floor.UpperLeftCorner;
	
	double CellWidth  = (double)floor.getWidth() / (double)numberCellsX;
	double CellHeight = (double)floor.getHeight() / (double)numberCellsY;
	double midX = CellWidth/2;
	double midY = CellHeight/2;

	core::dimension2df cellSize(CellWidth, CellHeight);
	
	core::vector2df nodePosition;
	int nodeContainingPoint = -1;
	
	graph_type::NodeIterator iter(graph);
	
	for(NavGraphNode<>* pN = iter.begin(); !iter.end(); pN=iter.next())
	{
		
		nodePosition = pN->getPosition();
		nodePosition.X -= midX;
		nodePosition.Y -= midY;
		rectf cell(nodePosition, cellSize);
		if(cell.isPointInside(position))
		{
			nodeContainingPoint = pN->getIndex();
			break;
		}
		
	}

	return nodeContainingPoint;
}